#include "Quaternion.h"
#include "Maths.h"

namespace Maths
{
const Quaternion Quaternion::Forward(0.0f,180.0f*(float)DEGTORAD,0.0f);
const Quaternion Quaternion::Backward(0.0f,0.0f,0.0f);
const Quaternion Quaternion::Left(0.0f,-90.0f*(float)DEGTORAD,0.0f);
const Quaternion Quaternion::Right(0.0f,90.0f*(float)DEGTORAD,0.0f);
const Quaternion Quaternion::Up(-90.0f*(float)DEGTORAD,0.0f,0.0f);
const Quaternion Quaternion::Down(90.0f*(float)DEGTORAD,0.0f, .0f);


Quaternion& Quaternion::operator+=( const Quaternion &b)
{
	W() += b.m_W;
	X() += b.m_X;
	Y() += b.m_Y;
	Z() += b.m_Z;
	return *this;
}

Quaternion& Quaternion::operator-=(const Quaternion &b)
{
	W() -= b.m_W;
	X() -= b.m_X;
	Y() -= b.m_Y;
	Z() -= b.m_Z;
	return *this;
}

Quaternion& operator+=( Quaternion &a, Quaternion &b)
{
	a.W() += b.W();
	a.X() += b.X();
	a.Y() += b.Y();
	a.Z() += b.Z();
	return a;
}

Quaternion& operator-=(Quaternion &a, Quaternion &b)
{
	a.W() -= b.W();
	a.X() -= b.X();
	a.Y() -= b.Y();
	a.Z() -= b.Z();
	return a;
}

Quaternion::Quaternion()
{
	Set(0, 0, 0);
}
Quaternion::Quaternion( float x, float y, float z )
{
	Set(x, y, z);
}

Quaternion::Quaternion( float x, float y, float z, float w )
{
	Set(x, y, z, w);
}

Quaternion::Quaternion( Vector3D& vector )
{
	Set(vector.X(), vector.Y(), vector.Z());
}

Quaternion::Quaternion( Matrix& m )
{
	const float diag = m(0,0) + m(1,1) + m(2,2) + 1;
	if( diag > 0.0f )
	{
		const float scale = sqrtf(diag) * 2.0f;

		m_X = (m(2, 1) - m(1, 2)) / scale;
		m_Y = (m(0, 1) - m(2, 0)) / scale;
		m_Z = (m(1, 1) - m(0, 1)) / scale;
		m_W = 0.25f * scale;
	} else
	{
		if( m(0, 0) > m(1, 1) && m(0, 0) > m(2, 2) )
		{
			const float scale = sqrtf( 1.0f + m(0, 0) - m(1, 1) - m(2, 2)) * 2.0f;

			m_X = 0.25f * scale;
			m_Y = (m(0, 1) + m(1, 0)) / scale;
			m_Z = (m(2, 0) + m(0, 2)) / scale;
			m_W = (m(2, 1) + m(1, 2)) / scale;
		} else if( m(1, 1) > m(2, 2) )
		{
			const float scale = sqrtf( 1.0f + m(1, 1) - m(0, 0) - m(2, 2)) * 2.0f;

			m_X = (m(0, 1) + m(1, 0)) / scale;
			m_Y = 0.25f * scale;
			m_Z = (m(1, 2) + m(2, 1)) / scale;
			m_W = (m(0, 2) + m(2, 0)) / scale;
		} else
		{
			const float scale = sqrtf( 1.0f + m(2, 2)) - m(0, 0) - m(1, 1) * 2.0f;

			m_X = (m(0, 2) + m(2, 0)) / scale;
			m_Y = (m(1, 2) + m(2, 1)) / scale;
			m_Z = 0.25f * scale;
			m_W = (m(1, 0) + m(0, 1)) / scale;
		}
	}
	Normalize();
}

bool Quaternion::operator==( const Quaternion& o ) const
{
	return ((m_X == o.m_X ) &&
		 (m_Y == o.m_Y ) &&
		 (m_Z == o.m_Z ) &&
		 (m_W == o.m_W ));
}

bool Quaternion::operator!=( const Quaternion& o ) const
{
	return !(*this == o);
}

Quaternion& Quaternion::operator=( const Quaternion& o )
{
	m_X = o.m_X;
	m_Y = o.m_Y;
	m_Z = o.m_Z;
	m_W = o.m_W;
	return *this;
}

Quaternion& Quaternion::operator=( Matrix& m )
{
	const float diag = m(0,0) + m(1,1) + m(2,2) + 1;
	if( diag > 0.0f )
	{
		const float scale = sqrtf(diag) * 2.0f;

		m_X = (m(2, 1) - m(1, 2)) / scale;
		m_Y = (m(0, 1) - m(2, 0)) / scale;
		m_Z = (m(1, 1) - m(0, 1)) / scale;
		m_W = 0.25f * scale;
	} else
	{
		if( m(0, 0) > m(1, 1) && m(0, 0) > m(2, 2) )
		{
			const float scale = sqrtf( 1.0f + m(0, 0) - m(1, 1) - m(2, 2)) * 2.0f;

			m_X = 0.25f * scale;
			m_Y = (m(0, 1) + m(1, 0)) / scale;
			m_Z = (m(2, 0) + m(0, 2)) / scale;
			m_W = (m(2, 1) + m(1, 2)) / scale;
		} else if( m(1, 1) > m(2, 2) )
		{
			const float scale = sqrtf( 1.0f + m(1, 1) - m(0, 0) - m(2, 2)) * 2.0f;

			m_X = (m(0, 1) + m(1, 0)) / scale;
			m_Y = 0.25f * scale;
			m_Z = (m(1, 2) + m(2, 1)) / scale;
			m_W = (m(0, 2) + m(2, 0)) / scale;
		} else
		{
			const float scale = sqrtf( 1.0f + m(2, 2)) - m(0, 0) - m(1, 1) * 2.0f;

			m_X = (m(0, 2) + m(2, 0)) / scale;
			m_Y = (m(1, 2) + m(2, 1)) / scale;
			m_Z = 0.25f * scale;
			m_W = (m(1, 0) + m(0, 1)) / scale;
		}
	}
	return Normalize();
}

Quaternion Quaternion::operator*( const Quaternion& o ) const
{
	Quaternion quat;
	quat.m_W = (m_W * o.m_W) - (m_X * o.m_X) - (m_Y * o.m_Y) - (m_Z * o.m_Z);
	quat.m_X = (m_W * o.m_X) + (m_X * o.m_W) + (m_Y * o.m_Z) - (m_Z * o.m_Y);
	quat.m_Y = (m_W * o.m_Y) - (m_X * o.m_Z) + (m_Y * o.m_W) + (m_Z * o.m_X);
	quat.m_Z = (m_W * o.m_Z) + (m_X * o.m_Y) - (m_Y * o.m_X) + (m_Z * o.m_W);
	return quat;
}

Quaternion Quaternion::operator*( const float o ) const
{
	Quaternion quat;
	quat.m_X = m_X * o;
	quat.m_Y = m_Y * o;
	quat.m_Z = m_Z * o;
	quat.m_W = m_W * o;
	return quat;
};

Quaternion& Quaternion::operator*=( float o )
{
	m_X *= o;
	m_Y *= o;
	m_Z *= o;
	m_W *= o;
	return *this;
};

Quaternion& Quaternion::operator*=( const Quaternion& o )
{
	return (*this = (*this) * o);
}

Quaternion Quaternion::operator+( const Quaternion& o ) const
{
	return Quaternion( m_X + o.m_X, m_Y + o.m_Y, m_Z + o.m_Z, m_W + o.m_W );
}

Matrix Quaternion::GetMatrix() const
{
	Matrix m;
	GetTransposedMatrix(m);
	return m;
}

void Quaternion::GetMatrix(  Matrix& dest, Vector3D& center ) const
{
	float * m = &(dest(0, 0));

	m[0] = 1.0f - 2.0f*m_Y*m_Y - 2.0f*m_Z*m_Z;
	m[1] = 2.0f*m_X*m_Y + 2.0f*m_Z*m_W;
	m[2] = 2.0f*m_X*m_Z - 2.0f*m_Y*m_W;
	m[3] = 0.0f;

	m[4] = 2.0f*m_X*m_Y - 2.0f*m_Z*m_W;
	m[5] = 1.0f - 2.0f*m_X*m_X - 2.0f*m_Z*m_Z;
	m[6] = 2.0f*m_Z*m_Y + 2.0f*m_X*m_W;
	m[7] = 0.0f;

	m[8] = 2.0f*m_X*m_Z + 2.0f*m_Y*m_W;
	m[9] = 2.0f*m_Z*m_Y - 2.0f*m_X*m_W;
	m[10] = 1.0f - 2.0f*m_X*m_X - 2.0f*m_Y*m_Y;
	m[11] = 0.0f;

	m[12] = center.X();
	m[13] = center.Y();
	m[14] = center.Z();
	m[15] = 1.0f;
}

void Quaternion::GetTransposedMatrix( Matrix &dest ) const
{
	float * m = &(dest(0, 0));

	m[0] = 1.0f - 2.0f*m_Y*m_Y - 2.0f*m_Z*m_Z;
	m[4] = 2.0f*m_X*m_Y + 2.0f*m_Z*m_W;
	m[8] = 2.0f*m_X*m_Z - 2.0f*m_Y*m_W;
	m[12] = 0.0f;

	m[1] = 2.0f*m_X*m_Y - 2.0f*m_Z*m_W;
	m[5] = 1.0f - 2.0f*m_X*m_X - 2.0f*m_Z*m_Z;
	m[9] = 2.0f*m_Z*m_Y + 2.0f*m_X*m_W;
	m[13] = 0.0f;

	m[2] = 2.0f*m_X*m_Z + 2.0f*m_Y*m_W;
	m[6] = 2.0f*m_Z*m_Y - 2.0f*m_X*m_W;
	m[10] = 1.0f - 2.0f*m_X*m_X - 2.0f*m_Y*m_Y;
	m[14] = 0.0f;

	m[3] = 0.0f;
	m[7] = 0.0f;
	m[11] = 0.0f;
	m[15] = 1.0f;
}

Quaternion& Quaternion::MakeInverse()
{
	m_X = -m_X; m_Y = -m_Y; m_Z = -m_Z;
	return *this;
}

Quaternion Quaternion::Inverse()
{
	return Quaternion(-m_X, -m_Y, -m_Z, m_W);
}

Quaternion& Quaternion::Set( float x, float y, float z, float w )
{
	m_X = x;
	m_Y = y;
	m_Z = z;
	m_W = w;
	return *this;
}

Quaternion& Quaternion::Set( float x, float y, float z ) 
{
	double angle;

	angle = x * 0.5;
	const double sr = sin(angle);
	const double cr = cos(angle);

	angle = y * 0.5;
	const double sp = sin(angle);
	const double cp = cos(angle);

	angle = z * 0.5;
	const double sy = sin(angle);
	const double cy = cos(angle);

	const double cpcy = cp * cy;
	const double spcy = sp * cy;
	const double cpsy = cp * sy;
	const double spsy = sp * sy;

	m_X = (float)(sr * cpcy - cr * spsy);
	m_Y = (float)(cr * spcy + sr * cpsy);
	m_Z = (float)(cr * cpsy - sr * spcy);
	m_W = (float)(cr * cpcy + sr * spsy);

	return Normalize();
}

Quaternion& Quaternion::Set( Vector3D& vec )
{
	return Set(vec.X(), vec.Y(), vec.Z());
}

Quaternion& Quaternion::Set(const Quaternion& o)
{
	return (*this = o);
}

bool Quaternion::Equals(const Quaternion& o, const float tolerance) const
{
	float x = m_X - o.m_X * (m_X - o.m_X < 0 ? -1 : 1);
	float y = m_Y - o.m_Y * (m_Y - o.m_Y < 0 ? -1 : 1);
	float z = m_Z - o.m_Z * (m_Z - o.m_Z < 0 ? -1 : 1);
	float w = m_W - o.m_W * (m_W - o.m_W < 0 ? -1 : 1);
	return x < tolerance &&
		   y < tolerance &&
		   z < tolerance &&
		   w < tolerance;
}

Quaternion& Quaternion::Normalize()
{
	float n = m_X*m_X + m_Y*m_Y + m_Z*m_Z + m_W*m_W;

	if (n == 1)
		return *this;

	n = 1.0f / sqrtf(n);
	return (*this *= n);
}


Quaternion& Quaternion::Lerp(Quaternion q1, Quaternion q2, float time)
{
	const float scale = 1.0f - time;
	const float invscale = time;
	return (*this = (q1 * scale) + (q2 * invscale));
}


Quaternion& Quaternion::Slerp(Quaternion q1, Quaternion q2, float time)
{
	float angle = q1.Dot(q2);

	// make sure we use the short rotation
	if (angle < 0.0f)
	{
		q1 *= -1.0f;
		angle *= -1.0f;
	}

	if (angle <= 0.95f) // spherical interpolation
	{
		const float theta = acosf(angle);
		const float invsintheta = (1.0f/(sinf(theta)));
		const float scale = sinf(theta * (1.0f-time)) * invsintheta;
		const float invscale = sinf(theta * time) * invsintheta;
		return (*this = (q1*scale) + (q2*invscale));
	}
	else // linear interploation
		return Lerp(q1,q2,time);
}


// calculates the dot product
float Quaternion::Dot(const Quaternion& o) const
{
	return (m_X * o.m_X) + (m_Y * o.m_Y) + (m_Z * o.m_Z) + (m_W * o.m_W);
}


//! axis must be unit length
//! angle in radians
Quaternion& Quaternion::FromAngleAxis(float angle, Vector3D& axis)
{
	const float fHalfAngle = 0.5f*angle;
	const float fSin = sinf(fHalfAngle);
	m_W = cosf(fHalfAngle);
	m_X = fSin*axis.X();
	m_Y = fSin*axis.Y();
	m_Z = fSin*axis.Z();
	return *this;
}


void Quaternion::ToAngleAxis(float& angle, Vector3D& axis) const
{
	const float scale = sqrtf(m_X*m_X + m_Y*m_Y + m_Z*m_Z);

	if (scale || m_W > 1.0f || m_W < -1.0f)
	{
		angle = 0.0f;
		axis.X() = 0.0f;
		axis.Y() = 1.0f;
		axis.Z() = 0.0f;
	}
	else
	{
		const float invscale = (1.0f/scale);
		angle = 2.0f * acosf(m_W);
		axis.X() = m_X * invscale;
		axis.Y() = m_Y * invscale;
		axis.Z() = m_Z * invscale;
	}
}

void Quaternion::ToEuler(Vector3D& euler) const
{
	const double sqw = m_W * m_W;
	const double sqx = m_X * m_X;
	const double sqy = m_Y * m_Y;
	const double sqz = m_Z * m_Z;
	const double test = 2.0 * (m_Y * m_W - m_X * m_Z);

	if (Maths::Equals(test, 1.0, Maths::EPSILON))
	{
		// heading = rotation about z-axis
		euler.Z() = (float) (-2.0*atan2(m_X, m_W));
		// bank = rotation about x-axis
		euler.X() = 0;
		// attitude = rotation about y-axis
		euler.Y() = (float) (Maths::PI/2.0);
	}
	else if (Maths::Equals(test, -1.0, Maths::EPSILON))
	{
		// heading = rotation about z-axis
		euler.Z() = (float) (2.0*atan2(m_X, m_W));
		// bank = rotation about x-axis
		euler.X() = 0;
		// attitude = rotation about y-axis
		euler.Y() = (float) (Maths::PI/-2.0);
	}
	else
	{
		// heading = rotation about z-axis
		euler.Z() = (float) atan2(2.0 * (m_X * m_Y + m_Z * m_W),(sqx - sqy - sqz + sqw));
		// bank = rotation about x-axis
		euler.X() = (float) atan2(2.0 * (m_Y * m_Z + m_X * m_W),(-sqx - sqy + sqz + sqw));
		// attitude = rotation about y-axis
		euler.Y() = (float) asin( Clamp(test, -1.0, 1.0) );
	}
}


Vector3D Quaternion::operator* (const Vector3D& v) const
{
	Maths::Vector3D q(m_X, m_Y, m_Z);
	return v+q.Cross(q.Cross(v) + (v*m_W))*2.0f;
}

Quaternion& Quaternion::MakeIdentity()
{
	m_W = 1.f;
	m_X = 0.f;
	m_Y = 0.f;
	m_Z = 0.f;
	return *this;
}

Quaternion& Quaternion::RotationFromTo(Vector3D& from, Vector3D& to)
{
	Vector3D v0 = from;
	Vector3D v1 = to;
	v0.Normalize();
	v1.Normalize();

	const float d = v0.Dot(v1);
	if (d >= 1.0f) // If dot == 1, vectors are the same
	{
		return MakeIdentity();
	}
	else if (d <= -1.0f) // exactly opposite
	{
		Vector3D axis(1.0f, 0.0f, 0.0f);
		axis = axis.Cross(Vector3D(m_X,m_Y,m_Z));
		if (axis.Length()==0)
		{
			axis.Set(0.f,1.f,0.f);
			axis.Cross(Vector3D(m_X,m_Y,m_Z));
		}
		return this->FromAngleAxis((float)Maths::PI, axis);
	}

	const float s = sqrtf( (1+d)*2 ); // optimize inv_sqrt
	const float invs = 1.f / s;
	Vector3D c = v0.Cross(v1)*invs;
	m_X = c.X();
	m_Y = c.Y();
	m_Z = c.Z();
	m_W = s * 0.5f;

	return *this;
}
}